/**
 * @file litefoc.c
 * @author wangshixin (gin0101@126.com)
 * @brief LiteFOC 执行文件
 * @version 1.0.0
 * @date 2023-09-28
 * 
 * @copyright Copyright (c) 2023
 * 
 */

#include "string.h"
#include "math.h"

#include "litefoc.h"
#include "litefoc_port.h"


#define LITEFOC_SIN_DATA_SIZE           1024    // sin数组子项数
#define LITEFOC_SIN_DATA_SIZE_1_3       341	    // sin数组子项数的1/3
#define LITEFOC_SIN_DATA_SIZE_1_4       256	    // sin数组子项数的1/4
#define LITEFOC_SIN_DATA_SIZE_2_4       512	    // sin数组子项数的2/4
#define LITEFOC_SIN_DATA_SIZE_3_4       768	    // sin数组子项数的3/4


/**
 * @brief 电机旋转方向
 * 
 */
typedef enum                            // 转动类型
{
    eMoveTypeAngle      = 0,            // 角度转
    eMoveTypeTime,                      // 时间转
} t_eMoveType;


/**
 * @brief litefoc控制参数结构
 * 
 */
typedef struct                          // litefoc控制结构体
{
    unsigned char   ucState;            // litefoc状态，0 - 不能运行，1 - 可以运行
    unsigned char   ucEnable;           // litefoc使能，0 - 停止运行，1 - 在运行

    float           fMagParAngle;       // 磁极间隔角度（两相邻磁极与中心点的夹角）
    unsigned long   ulMaxSpeed;         // 最大转速(度/秒)
    float           fCurAngle;          // 当前角度值，0~359.99

    float           fCurTorque;         // 当前力矩值，0~1.0

    float           fP;                 // 控制P值
    float           fI;                 // 控制I值
    float           fD;                 // 控制D值

    t_eMoveDir      eMoveDir;           // 电机旋转方向
    t_eMoveType     eMoveType;          // 转动类型，0 - 角度转，1 - 时间转
    unsigned long   ulSpeed;            // 转动速度，单位度每秒
    float           fTorque;            // 转动力矩，0~1.0
    float           fTargetAngle;       // 需要转动到的角度，单位度
    unsigned long   ulHoldTime;         // 转动时间，单位ms

    unsigned long   ulStartTime;        // 转动开始时间，单位ms
    unsigned long   ulCurTime;          // litefoc计时器，单位ms

    unsigned char   ucTaskPeriod;       // 任务调用周期，单位ms

    float           fInitAngle;         // 初始角度

    unsigned char   ucMode;             // 0 -  U 相索引增加角度增加，1 -  U 相索引增加角度减小
} _t_sLitefocCtl;

static _t_sLitefocCtl   _sLitefocCtl;   // lifefoc控制
static t_sLitefoc       _sLitefoc;      // 电机参数结构变量

/**
 * @brief sin 数据，该数据为一个完整 sin 周期数据的 1/4 ，这样可以节省单片机空间
 * 
 */
static const unsigned char _ucSinData[LITEFOC_SIN_DATA_SIZE_1_4 + 1] = {
0x80, 0x81, 0x82, 0x82, 0x83, 0x84, 0x85, 0x85, 0x86, 0x87, 0x88, 0x89, 0x89, 0x8a, 0x8b, 0x8c, 0x8c, 0x8d, 0x8e, 0x8f, 0x90, 0x90, 0x91, 0x92, 0x93, 0x93, 0x94, 0x95, 0x96, 0x96, 0x97, 0x98, 0x99, 0x9a, 0x9a, 0x9b, 
0x9c, 0x9d, 0x9d, 0x9e, 0x9f, 0xa0, 0xa0, 0xa1, 0xa2, 0xa3, 0xa3, 0xa4, 0xa5, 0xa6, 0xa6, 0xa7, 0xa8, 0xa9, 0xa9, 0xaa, 0xab, 0xac, 0xac, 0xad, 0xae, 0xae, 0xaf, 0xb0, 0xb1, 0xb1, 0xb2, 0xb3, 0xb3, 0xb4, 0xb5, 0xb6, 
0xb6, 0xb7, 0xb8, 0xb8, 0xb9, 0xba, 0xba, 0xbb, 0xbc, 0xbd, 0xbd, 0xbe, 0xbf, 0xbf, 0xc0, 0xc1, 0xc1, 0xc2, 0xc3, 0xc3, 0xc4, 0xc5, 0xc5, 0xc6, 0xc7, 0xc7, 0xc8, 0xc8, 0xc9, 0xca, 0xca, 0xcb, 0xcc, 0xcc, 0xcd, 0xce, 
0xce, 0xcf, 0xcf, 0xd0, 0xd1, 0xd1, 0xd2, 0xd2, 0xd3, 0xd4, 0xd4, 0xd5, 0xd5, 0xd6, 0xd6, 0xd7, 0xd8, 0xd8, 0xd9, 0xd9, 0xda, 0xda, 0xdb, 0xdb, 0xdc, 0xdd, 0xdd, 0xde, 0xde, 0xdf, 0xdf, 0xe0, 0xe0, 0xe1, 0xe1, 0xe2, 
0xe2, 0xe3, 0xe3, 0xe4, 0xe4, 0xe5, 0xe5, 0xe6, 0xe6, 0xe6, 0xe7, 0xe7, 0xe8, 0xe8, 0xe9, 0xe9, 0xea, 0xea, 0xea, 0xeb, 0xeb, 0xec, 0xec, 0xed, 0xed, 0xed, 0xee, 0xee, 0xef, 0xef, 0xef, 0xf0, 0xf0, 0xf0, 0xf1, 0xf1, 
0xf1, 0xf2, 0xf2, 0xf2, 0xf3, 0xf3, 0xf3, 0xf4, 0xf4, 0xf4, 0xf5, 0xf5, 0xf5, 0xf6, 0xf6, 0xf6, 0xf6, 0xf7, 0xf7, 0xf7, 0xf8, 0xf8, 0xf8, 0xf8, 0xf9, 0xf9, 0xf9, 0xf9, 0xfa, 0xfa, 0xfa, 0xfa, 0xfa, 0xfb, 0xfb, 0xfb, 
0xfb, 0xfb, 0xfc, 0xfc, 0xfc, 0xfc, 0xfc, 0xfc, 0xfd, 0xfd, 0xfd, 0xfd, 0xfd, 0xfd, 0xfd, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
0xff, 0xff, 0xff, 0xff, 0xff
};

static float _fLastAngle        = 0.0f;     // 上一次角度值
static float _fLastAngleErr    	= 0.0f;     // 上一次角度误差
static float _fAngleErrSum      = 0.0f;     // 角度误差累积

static float _fkp               = 0.0f;
static float _fki    	        = 0.0f;
static float _fkd               = 0.0f;

/********************************************** 内部函数定义 BEGIN **********************************************/
/**
 * @brief 根据索引找到对应正弦值
 * 
 * @param usIndex 索引值
 * @return 对应正弦值 
 */
static unsigned char _litefoc_get_data(unsigned short usIndex)
{
    unsigned char data = 0;

    if (usIndex >= LITEFOC_SIN_DATA_SIZE_3_4)
        data = 0xFF - _ucSinData[LITEFOC_SIN_DATA_SIZE - usIndex];
    else if (usIndex >= LITEFOC_SIN_DATA_SIZE_2_4)
        data = 0xFF - _ucSinData[usIndex - LITEFOC_SIN_DATA_SIZE_2_4];
    else if (usIndex >= LITEFOC_SIN_DATA_SIZE_1_4)
        data = _ucSinData[LITEFOC_SIN_DATA_SIZE_2_4 - usIndex];
    else
        data = _ucSinData[usIndex];

    return data;
}
/********************************************** 内部函数定义 END ************************************************/

/**
 * @brief 初始化
 * 
 * @return  0 - 成功, 其他 - 失败
 */
unsigned char litefoc_init(t_sLitefoc *sLitefoc)
{
    unsigned char res = 0;

    if (sLitefoc == NULL) return 1;
    if (sLitefoc->ucN < 3) return 2;
    if (sLitefoc->ucP < 3) return 3;
    if (sLitefoc->fPowerVolt < LITEFOC_MIN_POWER_VOLT) return 4;
	if (sLitefoc->usKV <= 0) return 5;
    if (sLitefoc->ucPeriod < 1) return 6;

    litefoc_port_init();
    litefoc_port_set_pwm(0, 0, 0);
    litefoc_port_power(1);

    memcpy((void*)&_sLitefoc, (void*)sLitefoc, sizeof(t_sLitefoc));
	
    // 初始化litefoc控制参数
    memset((void*)&_sLitefocCtl, 0, sizeof(_t_sLitefocCtl));

	_sLitefocCtl.fMagParAngle   = 360.0f / (float)(_sLitefoc.ucP >> 1);
	_sLitefocCtl.ulMaxSpeed     = _sLitefoc.fPowerVolt * (float)_sLitefoc.usKV * 360.0f / 60.0f;
    _sLitefocCtl.fCurAngle     	= 0.0f;

    _sLitefocCtl.eMoveDir       = eClockwise;
    _sLitefocCtl.fTargetAngle	= 0.0f;
    _sLitefocCtl.ulSpeed        = 0;
    _sLitefocCtl.fTorque        = 0.0f;
    _sLitefocCtl.ulHoldTime     = 0;

    _sLitefocCtl.ulStartTime    = 0;
    _sLitefocCtl.ulCurTime      = 0;
    _sLitefocCtl.ucTaskPeriod   = sLitefoc->ucPeriod;

    _sLitefocCtl.ucEnable       = 0;
    _sLitefocCtl.ucState        = 1;

    _sLitefocCtl.ucMode         = 0;

    _fkp = LITEFOC_DEFAULT_PID_KP;
    _fki = LITEFOC_DEFAULT_PID_KI;
    _fkd = LITEFOC_DEFAULT_PID_KD;

    // 电机角度调整，找到角度值与电机三相索引值对应关系，调整时请保持电机空载。
    // 由于三相信号相位相差120度，所以只需知道一相对应关系就行，这里选择 U 相。
    // 把 U 相初始值设置为 0 ，然后找到对应电机角度，把该角度设置为初始角度。
    do
    {
        unsigned short uIndex = 0;
        unsigned short vIndex = 0;
        unsigned short wIndex = 0;

        unsigned char uValue = 0;
        unsigned char vValue = 0;
        unsigned char wValue = 0;

        float torque = 0.01f;
        unsigned long lastTime = 0;

        vIndex = (uIndex + LITEFOC_SIN_DATA_SIZE_1_3) % LITEFOC_SIN_DATA_SIZE;
        wIndex = (vIndex + LITEFOC_SIN_DATA_SIZE_1_3) % LITEFOC_SIN_DATA_SIZE;

        // 转换索引值，使其能对应到_ucSinData数组中的值
        uValue = _litefoc_get_data(uIndex);
        vValue = _litefoc_get_data(vIndex);
        wValue = _litefoc_get_data(wIndex);

        // 标定 U 相索引为 0 对应的角度位置
        while (torque <= 0.2f)
        {
            litefoc_port_set_pwm(uValue * torque, vValue * torque, wValue * torque);
			
			if (_sLitefocCtl.ulCurTime - lastTime < 20)
                continue;
			lastTime = _sLitefocCtl.ulCurTime;
			
            torque += 0.01f;
        }
        // 记录初始化角度
        litefoc_port_get_angle(&_sLitefocCtl.fCurAngle);
        _sLitefocCtl.fInitAngle = _sLitefocCtl.fCurAngle;

        // 标定 Mode 值
        unsigned char i = 0;
        while (i < 100)
        {
            uIndex = i;
            vIndex = (uIndex + LITEFOC_SIN_DATA_SIZE_1_3) % LITEFOC_SIN_DATA_SIZE;
            wIndex = (vIndex + LITEFOC_SIN_DATA_SIZE_1_3) % LITEFOC_SIN_DATA_SIZE;

            // 转换索引值，使其能对应到_ucSinData数组中的值
            uValue = _litefoc_get_data(uIndex);
            vValue = _litefoc_get_data(vIndex);
            wValue = _litefoc_get_data(wIndex);
            litefoc_port_set_pwm(uValue * torque, vValue * torque, wValue * torque);

			if (_sLitefocCtl.ulCurTime - lastTime < 10)
                continue;
            lastTime = _sLitefocCtl.ulCurTime;
			
            i += 10;
        }
        litefoc_port_get_angle(&_sLitefocCtl.fCurAngle);
        float angleErr = _sLitefocCtl.fCurAngle - _sLitefocCtl.fInitAngle;
        if (((angleErr < 0.0f) && (angleErr > -180.0f)) || (angleErr > 180.0f))
            _sLitefocCtl.ucMode = 1;
    } while (0);

    // 释放电机
    litefoc_port_set_pwm(0, 0, 0);
	litefoc_port_power(0);
    
    return res;
}


/**
 * @brief 设置供电电压
 * 
 * @param   fVolt 电机供电电压，最小 1V
 * 
 * @return  0 - 设置成功, 其他 - 设置失败
 */
unsigned char litefoc_set_volt(float fVolt)
{
    unsigned char res = 0;

    if (fVolt < LITEFOC_MIN_POWER_VOLT) res = 1;
    else
    {
        _sLitefoc.fPowerVolt    = fVolt;
        _sLitefocCtl.ulMaxSpeed = _sLitefoc.fPowerVolt * (float)_sLitefoc.usKV * 360.0f / 60.0f;;
    }

    return res;
}


/**
 * @brief 获取当前角度
 * 
 * @return  角度值
 */
float litefoc_get_angle(void)
{
    return _sLitefocCtl.fCurAngle;
}


/**
 * @brief 获取当前力矩
 * 
 * @return  力矩值（值被归一化）
 */
float litefoc_get_torque(void)
{
    return _sLitefocCtl.fCurTorque;
}


/**
 * @brief 配置PID值
 * 
 * @param fP    P值
 * @param fI    I值
 * @param fD    D值
 */
void litefoc_set_pid(float fP, float fI, float fD)
{
    _fkp = fP;
    _fki = fI;
    _fkd = fD;
}


/**
 * @brief foc 循环执行
 * 
 * @return  0 - 执行成功，1 - 未开启，2 - 未到间隔时间，3 - 获取当前电机角度失败，4 - 旋转时间已结束
 */
unsigned char litefoc_loop(void)
{
    unsigned char res = 0;
    static unsigned long lastTime = 0;

    if (_sLitefocCtl.ucEnable == 0) return 1;
    
    // 两次间隔时间为任务调用周期
    if (_sLitefocCtl.ulCurTime - lastTime < _sLitefocCtl.ucTaskPeriod) return 2;
    lastTime = _sLitefocCtl.ulCurTime;

    if (litefoc_port_get_angle(&_sLitefocCtl.fCurAngle) != 0) return 3;

    // 旋转时间处理
    if (_sLitefocCtl.eMoveType == eMoveTypeTime)
    {
        if (_sLitefocCtl.ulCurTime - _sLitefocCtl.ulStartTime >= _sLitefocCtl.ulHoldTime)
        {
            litefoc_port_set_pwm(0, 0, 0);
            litefoc_port_power(0);
            return 4;
        }
    }

    /**************************************** 计算电机三相值 BEGIN ****************************************/
    float torque = 0.0f;    // 期望的力矩
    float angle = 0.0f;     // 期望的角度
    float angleErr = 0.0f;  // 期望角度与真实差

    float dis = (_sLitefocCtl.ucTaskPeriod * _sLitefocCtl.ulSpeed) / 1000.0f;

    // 电机三相索引值
    unsigned short sinDataUIndex = 0;
	unsigned short sinDataVIndex = 0;
	unsigned short sinDataWIndex = 0;

    // 计算当前角度与初始化角度 U 相偏差值
    float angleOffset = _sLitefocCtl.fCurAngle - _sLitefocCtl.fInitAngle;
    unsigned short uIndexOffset = 0;
    if (_sLitefocCtl.ucMode)
    {
        if (angleOffset <= 0.0f) angleOffset *= -1.0f;
        else angleOffset = 360.0f - angleOffset;
        uIndexOffset = (unsigned short)(angleOffset / _sLitefocCtl.fMagParAngle * LITEFOC_SIN_DATA_SIZE) % LITEFOC_SIN_DATA_SIZE;
    }
    else
    {
        if (angleOffset >= 0.0f) angleOffset *= -1.0f;
        else angleOffset = 360.0f + angleOffset;
        uIndexOffset = (unsigned short)(angleOffset / _sLitefocCtl.fMagParAngle * LITEFOC_SIN_DATA_SIZE) % LITEFOC_SIN_DATA_SIZE;
    }

    // 转动到目标角度
    if (_sLitefocCtl.eMoveType == eMoveTypeAngle)
    {
        float angleDiff = _sLitefocCtl.fCurAngle - _sLitefocCtl.fTargetAngle;
        unsigned short uIndexDiff = 0;

        if (angleDiff < -180.0f) angleDiff = 360.0f + angleDiff;
        else if (angleDiff > 180.0f) angleDiff = angleDiff - 360.0f;

        if (fabs(angleDiff) < 2.0f)
            _fAngleErrSum = 0.0f;

        // 根据旋转方向计算出电机 U 相索引值，90度是最大力矩臂
        uIndexDiff = (float)LITEFOC_SIN_DATA_SIZE_1_4 * (fabs(angleDiff) / 15.0f + 0.02f);
        uIndexDiff = uIndexDiff > LITEFOC_SIN_DATA_SIZE_1_4 ? LITEFOC_SIN_DATA_SIZE_1_4 : uIndexDiff;
        if (angleDiff > 0.0f)
        {
            if (_sLitefocCtl.ucMode)
                uIndexOffset += uIndexDiff;
            else
                uIndexOffset += (LITEFOC_SIN_DATA_SIZE - uIndexDiff);
        }
        else
        {
            if (_sLitefocCtl.ucMode)
                uIndexOffset += (LITEFOC_SIN_DATA_SIZE - uIndexDiff);
            else
                uIndexOffset += uIndexDiff;
        }

        // 计算期望角度
        if (angleDiff > 0.0f)
            angle = _fLastAngle - dis;
        else
            angle = _fLastAngle + dis;
        
        // 计算角度误差
        angleErr = angle - _sLitefocCtl.fCurAngle;
        if (angleDiff > 0.0f)
            angleErr = -1.0f * angleErr;
    }
    // 转动设定时长
    else
    {
        // 根据旋转方向计算出电机 U 相索引值，90度是最大力矩臂
        if (_sLitefocCtl.eMoveDir == eClockwise) uIndexOffset += LITEFOC_SIN_DATA_SIZE_1_4;
        else uIndexOffset += (LITEFOC_SIN_DATA_SIZE - LITEFOC_SIN_DATA_SIZE_1_4);

        // 计算期望角度
        if (_sLitefocCtl.eMoveDir == eClockwise)
            angle = _fLastAngle - dis;
        else
            angle = _fLastAngle + dis;

        // 计算角度误差
        angleErr = angle - _sLitefocCtl.fCurAngle;
        if (_sLitefocCtl.eMoveDir == eClockwise)
            angleErr = -1.0f * angleErr;
    }

    sinDataUIndex   = uIndexOffset % LITEFOC_SIN_DATA_SIZE;
    sinDataVIndex   = (sinDataUIndex + LITEFOC_SIN_DATA_SIZE_1_3) % LITEFOC_SIN_DATA_SIZE;
    sinDataWIndex   = (sinDataVIndex + LITEFOC_SIN_DATA_SIZE_1_3) % LITEFOC_SIN_DATA_SIZE;

    if (angle >= 360.0f) angle -= 360.0f;
    if (angle < 0.0f) angle += 360.0f;

    if (angleErr < -180.0f) angleErr = 360.0f + angleErr;
    else if (angleErr > 180.0f) angleErr = 360.0f - angleErr;

    // 力矩分量
    float torque_p = _sLitefocCtl.fP * fabs(angleErr);
    float torque_i = _sLitefocCtl.fI * _fAngleErrSum;
    float torque_d = _sLitefocCtl.fD * (angleErr - _fLastAngleErr);

    // 计算角度误差积
    if (fabs(torque_i) < _sLitefocCtl.fTorque)
    {
        if ((torque_i > 0.0f && angleErr < -1.0f) || (torque_i < 0.0f && angleErr > 1.0f))
            _fAngleErrSum = angleErr;
        else
            _fAngleErrSum += angleErr;
    }
    else if ((torque_i <= -_sLitefocCtl.fTorque && angleErr > 0.0f) || (torque_i >= _sLitefocCtl.fTorque && angleErr < 0.0f))
        _fAngleErrSum = angleErr;
    torque_i = _sLitefocCtl.fI * _fAngleErrSum;
    if (torque_i > _sLitefocCtl.fTorque) torque_i = _sLitefocCtl.fTorque;
    if (torque_i < -_sLitefocCtl.fTorque) torque_i = -_sLitefocCtl.fTorque;

    // PID 计算力矩
    torque = torque_p + torque_i + torque_d;
    torque = torque < 0.0f ? 0.0f : torque;
    torque = torque > _sLitefocCtl.fTorque ? _sLitefocCtl.fTorque : torque;
    _sLitefocCtl.fCurTorque = torque;

    // 转换索引值，使其能对应到_ucSinData数组中的值
    unsigned char u = _litefoc_get_data(sinDataUIndex);
    unsigned char v = _litefoc_get_data(sinDataVIndex);
    unsigned char w = _litefoc_get_data(sinDataWIndex);
    /**************************************** 计算电机三相值 END ****************************************/
    
    // 调整 PWM 值
    litefoc_port_set_pwm(u * torque, v * torque, w * torque);

    _fLastAngle     = _sLitefocCtl.fCurAngle;
    _fLastAngleErr  = angleErr;

    return res;
}


/**
 * @brief foc任务执行中断调用函数
 * 
 */
void litefoc_task_handler(void)
{
    if (_sLitefocCtl.ucState)
        _sLitefocCtl.ulCurTime += _sLitefocCtl.ucTaskPeriod;
}


/**
 * @brief 电机转动角度控制
 * 
 * @param   fAngle 转动角度，范围 0.0 ~ 360.0f，角度为绝对位置，单位度
 * @param   unSpeed 转动速度，单位度每秒
 * @param   fTorque 转动力矩，0~1.0
 * 
 * @return  0 - 设置成功, 其他 - 设置失败
 */
unsigned char litefoc_angle(float fAngle, unsigned long ulSpeed, float fTorque)
{
    if (_sLitefocCtl.ucState == 0) return 1;
    if (fAngle < 0.0f || fAngle > 360.0f) return 2;
    if (fTorque <= 0.0f) return 3;

    if (fTorque > 1.0f) fTorque = 1.0f;
    if (fAngle == 360.0f) fAngle = 359.999f;
    if (ulSpeed > _sLitefocCtl.ulMaxSpeed) ulSpeed = _sLitefocCtl.ulMaxSpeed;

    _sLitefocCtl.eMoveType  = eMoveTypeAngle;
    _sLitefocCtl.ulSpeed    = ulSpeed;
    _sLitefocCtl.fTorque    = fTorque;
    _sLitefocCtl.fTargetAngle= fAngle;

    _sLitefocCtl.fP         = _fkp;
    _sLitefocCtl.fI         = _fki;
    _sLitefocCtl.fD         = _fkd;

    // 获取当前角度
    litefoc_port_get_angle(&_sLitefocCtl.fCurAngle);

    _sLitefocCtl.ucEnable   = 1;

    _fLastAngle  	= _sLitefocCtl.fCurAngle;
	_fLastAngleErr	= 0.0f;
    _fAngleErrSum   = 0.0f;

    litefoc_port_power(1);

	return 0;
}


/**
 * @brief 电机转动控制
 * 
 * @param   eMoveDir 电机旋转方向，参考枚举t_eMoveDir
 * @param   unSpeed 转动速度，单位度每秒
 * @param   fTorque 转动力矩，0~1.0
 * @param   snTime 转动时间，为负数时时间无从大，单位ms
 * 
 * @return  0 - 设置成功, 其他 - 设置失败
 */
unsigned char litefoc_move(t_eMoveDir eMoveDir, unsigned long ulSpeed, float fTorque, unsigned long ulTime)
{
    if (_sLitefocCtl.ucState == 0) return 1;
    if (eMoveDir != eClockwise && eMoveDir != eAnticlockwise) return 2;
    if (fTorque <= 0.0f) return 3;

    if (fTorque > 1.0f) fTorque = 1.0f;
    if (ulSpeed > _sLitefocCtl.ulMaxSpeed) ulSpeed = _sLitefocCtl.ulMaxSpeed;

    _sLitefocCtl.eMoveDir   = eMoveDir;
    _sLitefocCtl.eMoveType  = eMoveTypeTime;
    _sLitefocCtl.ulSpeed    = ulSpeed;
    _sLitefocCtl.fTorque    = fTorque;
    _sLitefocCtl.ulHoldTime	= ulTime;

    _sLitefocCtl.fP         = _fkp;
    _sLitefocCtl.fI         = _fki;
    _sLitefocCtl.fD         = _fkd;

    // 获取当前角度
    litefoc_port_get_angle(&_sLitefocCtl.fCurAngle);

    _sLitefocCtl.ulStartTime= _sLitefocCtl.ulCurTime;
    _sLitefocCtl.ucEnable   = 1;

    _fLastAngle  	= _sLitefocCtl.fCurAngle;
	_fLastAngleErr	= 0.0f;
    _fAngleErrSum   = 0.0f;

    litefoc_port_power(1);

	return 0;
}


/**
 * @brief 电机暂停转动
 * 
 */
void litefoc_pause(void)
{
    _sLitefocCtl.ucEnable = 0;
}


/**
 * @brief 电机继续转动
 * 
 * @return  0 - 恢复成功, 其他 - 恢复失败
 */
unsigned char litefoc_resume(void)
{
    unsigned char res = 0;

    if (_sLitefocCtl.ulSpeed)
        _sLitefocCtl.ucEnable = 1;
    else
        res = 1;
    
    return res;
}


/**
 * @brief 电机停止转动
 * 
 */
void litefoc_stop(void)
{
    _sLitefocCtl.ucEnable   = 0;

    _sLitefocCtl.ulSpeed    = 0;
    _sLitefocCtl.fTorque    = 0.0f;
    _sLitefocCtl.ulHoldTime = 0;

    litefoc_port_set_pwm(0, 0, 0);
    litefoc_port_power(0);
}
